#include "canthread.h".h"
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <unistd.h>
#include <cstring>
#include <QDebug>
#include <QElapsedTimer>
#include <fcntl.h>
#include "FloraLogger.h"
CanThread::CanThread(const QString &interfaceName, QObject *parent)
    : QThread(parent), canSocket(-1), interfaceName(interfaceName), stopThread(false) {

    m_isRun = false;
}

CanThread::~CanThread() {
    stopThread = true;
    wait(); // 等待线程退出

    if (canSocket >= 0) {
        ::close(canSocket);
    }
}

bool CanThread::initCanDevice(const QString &interfaceName) {
    struct sockaddr_can addr;
    struct ifreq ifr;

    // 创建 CAN 套接字
    canSocket = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (canSocket < 0) {
        qCritical() << "Failed to create CAN socket:" << strerror(errno);
        return false;
    }

    // 设置 CAN 接口名称（如 can0）
    strncpy(ifr.ifr_name, interfaceName.toStdString().c_str(), IFNAMSIZ - 1);
    if (ioctl(canSocket, SIOCGIFINDEX, &ifr) < 0) {
        qCritical() << "Failed to get interface index for" << interfaceName << ":" << strerror(errno);
        ::close(canSocket);
        return false;
    }
    int flags = fcntl(canSocket,F_GETFL,0);
    fcntl(canSocket,F_SETFL,flags | O_NONBLOCK);
    // 绑定 CAN 套接字到指定接口
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    if (bind(canSocket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        qCritical() << "Failed to bind CAN socket:" << strerror(errno);
        ::close(canSocket);
        return false;
    }
    struct can_filter filter[2];
    filter[0].can_id = 0x500;
    filter[0].can_mask =0x700;
    filter[1].can_id = 0x600;
    filter[1].can_mask =0x700;
    setsockopt(canSocket,SOL_CAN_RAW,CAN_RAW_FILTER,&filter,sizeof(filter));


    int bufszie = 524288;
    if(setsockopt(canSocket,SOL_SOCKET,SO_RCVBUF,&bufszie,sizeof(bufszie))>=0)
    {
        LOG_DEBUG<<"buff set ok";
    }

    qDebug()<<interfaceName<<"初始化完成";
    return true;
}

void CanThread::parseCanMessage( struct can_frame &frame)
{
    // 移动量
    float X_Trans;
    float Y_Trans;
    float Z_Trans;
    float yaw,pitch;
    LOG_DEBUG<<"reciave:ID:0x"<<QString::number( frame.can_id,16)<<QByteArray((char *)frame.data,8).toHex(' ');
    switch (frame.can_id) {
    case 0x300:
        X_Trans = (frame.data[4] <<8 | frame.data[5])/10.0;
        Y_Trans = (frame.data[2] <<8 | frame.data[3])/100.0;
        Z_Trans = (frame.data[0] <<8 | frame.data[1])/10.0;
        emit MCU_Trans(X_Trans,Y_Trans,Z_Trans);
        break;
    case 0x301:
        yaw =  (frame.data[0] <<8 | frame.data[1])/1000.0;
        pitch =  (frame.data[2] <<8 | frame.data[3])/1000.0;
        emit MCU_Rot(yaw,pitch);
        break;
    default:
        break;
    }
}





void CanThread::enqueueMessage(const struct can_frame &frame) {
    if(m_isRun == false)return;
    QMutexLocker locker(&queueMutex);
   // LOG_DEBUG<<messageQueue.length();
    messageQueue.enqueue(frame);
}

void CanThread::enqueueMessage_1( struct can_frame frame)
{
    enqueueMessage(frame);
}

bool CanThread::sendAttitude(float yaw, float pitch, float roll)
{
    int16_t yaw_16 = yaw*1000;
    int16_t pitch_16 = pitch*1000;
    int16_t roll_16 = roll*1000;
    struct can_frame frame;
    memset(&frame, 0, sizeof(frame));
    frame.can_id = 0x101;
    frame.can_dlc = 6; // 数据长度为 6 字节 (3 个 int16)
    // 填充数据
   frame.data[0] = (yaw_16 >> 8) & 0xFF;  // 航向角高字节
   frame.data[1] = yaw_16 & 0xFF;         // 航向角低字节
   frame.data[2] = (pitch_16 >> 8) & 0xFF;    // 俯仰角高字节
   frame.data[3] = pitch_16 & 0xFF;           // 俯仰角低字节
   frame.data[4] = (roll_16 >> 8) & 0xFF;     // 横滚角高字节
   frame.data[5] = roll_16 & 0xFF;            // 横滚角低字节
   enqueueMessage(frame);

   return true;
}

bool CanThread::sendOffset(float x_offset, float y_offset, float z_offset, bool CamHkAck)
{
    // 将偏移量乘以 100 转换为 int16_t 类型
    int16_t x_offset_16 = static_cast<int16_t>(x_offset * 100);
    int16_t y_offset_16 = static_cast<int16_t>(y_offset * 100);
    int16_t z_offset_16 = static_cast<int16_t>(z_offset * 100);

    // 创建 CAN 帧
    struct can_frame frame;
    memset(&frame, 0, sizeof(frame));
    frame.can_id = 0x100;  // 设置 CAN 消息 ID
    frame.can_dlc = 6;     // 数据长度为 6 字节 (3 个 int16)

    // 填充数据
    frame.data[0] = (x_offset_16 >> 8) & 0xFF;  // X 偏移高字节
    frame.data[1] = x_offset_16 & 0xFF;         // X 偏移低字节
    frame.data[2] = (y_offset_16 >> 8) & 0xFF;  // Y 偏移高字节
    frame.data[3] = y_offset_16 & 0xFF;         // Y 偏移低字节
    frame.data[4] = (z_offset_16 >> 8) & 0xFF;  // Z 偏移高字节
    frame.data[5] = z_offset_16 & 0xFF;         // Z 偏移低字节
    frame.data[6] =CamHkAck;
    enqueueMessage(frame);
    return true;
}

void CanThread::run() {
    // 初始化 CAN 设备
    if (!initCanDevice(interfaceName)) {
        return;
    }
    m_isRun = true;
    while (!stopThread) {
        // **检查是否有消息需要发送**
        queueMutex.lock();
        if (!messageQueue.isEmpty()) {
            struct can_frame frame = messageQueue.dequeue();
            queueMutex.unlock();
            // 发送 CAN 消息
            if (write(canSocket, &frame, sizeof(frame)) != sizeof(frame)) {
                qDebug() << "Failed to send CAN frame:" << strerror(errno);
            } else {
                //qDebug() << "Sent frame: ID =" << hex << frame.can_id <<QByteArray((char *)frame.data,8).toHex(' ') ;
            }
        } else {
            queueMutex.unlock();
        }
        QThread::msleep(3);
        // **检查是否有返回的 CAN 消息**
        struct can_frame receivedFrame;
        while(1)
        {

            int nbytes = read(canSocket, &receivedFrame, sizeof(receivedFrame));
            //LOG_DEBUG<<"read message"<<nbytes;
            if (nbytes > 0) {
                //parseCanMessage(receivedFrame);
                emit messageReceived(receivedFrame); // 发出信号
            } else if (nbytes < 0 ) {
                if(errno == EAGAIN)
                {
                    //LOG_DEBUG<< interfaceName<<"no message";
                }
                else
                {
                    LOG_DEBUG<< interfaceName << "Error reading CAN frame:" << strerror(errno);

                }

               break;
            }else if(nbytes == 0)
            {

                LOG_DEBUG<< interfaceName <<"no can message";
                break;
            }
        }

        //LOG_DEBUG<<"READ_结束";;
        // **防止过度占用 CPU，增加一点延迟**
        QThread::msleep(2);
    }
    m_isRun = false;
}
